# !/usr/bin/python
#
# Example code to go through the hokuyo_30m.bin file, read timestamps and the hits
# in each packet, and plot them.
#
# To call:
#
#   python read_hokuyo_30m.py hokuyo_30m.bin
#   python read_hokuyo_30m.py /media/gwm/152E776914CDED89/linux_work/open_dataset/nclt/20130110/hokuyo_30m.bin
#

import sys
import struct
import numpy as np
import matplotlib.pyplot as plt

def convert(x_s, scaling=0.005, offset=-100.0):
    """将原始测量值转换为实际距离(m)"""
    return x_s * scaling + offset

def main(args):
    if len(args) < 2:
        print("Usage: python read_hokuyo_30m.py <input.bin>")
        return 1

    # 激光雷达参数配置
    NUM_HITS = 1081
    RAD_START = np.deg2rad(-135)  # 起始角度(-135度)
    RAD_STEP = np.deg2rad(0.25)   # 角度步长(0.25度)
    
    # 预计算所有角度
    angles = np.linspace(RAD_START, 
                        RAD_START + (NUM_HITS-1)*RAD_STEP, 
                        NUM_HITS)

    try:
        with open(args[1], "rb") as f_bin:
            plt.ion()
            fig = plt.figure()
            
            while True:
                # 读取时间戳(8字节)
                timestamp = f_bin.read(8)
                if not timestamp:  # 文件结束
                    break
                    
                utime = struct.unpack('<Q', timestamp)[0]
                print(f'Timestamp: {utime}')

                # 读取测量数据(每个2字节)
                ranges = np.fromfile(f_bin, dtype='<H', count=NUM_HITS)

                if len(ranges) != NUM_HITS:  # 数据不完整
                    break
                    
                # 转换为实际距离
                distances = convert(ranges)

                # 转换为笛卡尔坐标
                x = distances * np.cos(angles)
                y = distances * np.sin(angles)
                # print(type(x))  # 输出：<class 'numpy.ndarray'>
                # print(x.shape)  # 输出如：(1081,) 

                # 可视化
                plt.clf()
                plt.plot(x, y, '.', markersize=5)
                plt.title(f'Scan at {utime}')
                plt.axis('equal')
                plt.draw()
                plt.pause(0.1)

    except FileNotFoundError:
        print(f"Error: File {args[1]} not found")
        return 1
    except struct.error:
        print("Error: Invalid binary data format")
        return 1

    return 0

if __name__ == '__main__':
    sys.exit(main(sys.argv))
